// Heuristic.cpp: implementation of the CHeuristic class.
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"
#include "IAgent.h"
#include "Heuristic.h"
#include "Graph.h"
#include "AIObject.h"
#include "Cry_Math.h"
#include "AILog.h"
#include "CAISystem.h"
#include "VolumeNavRegion.h"
#include "Navigation/CustomNavRegion.h"

#include <limits>


//====================================================================
// CStandardHeuristic
//====================================================================
CStandardHeuristic::CStandardHeuristic()
{
  SetProperties(m_properties);
	SetStartEndData(0, Vec3Constants<float>::fVec3_Zero, 0, Vec3Constants<float>::fVec3_Zero, PathfindingExtraConstraints());
}

CStandardHeuristic::~CStandardHeuristic()
{
}
	

//====================================================================
// SetStartEndData
//====================================================================
void CStandardHeuristic::SetStartEndData(const GraphNode *pStartNode, const Vec3 &startPos, const GraphNode *pEndNode, const Vec3 &endPos, const PathfindingExtraConstraints &extraConstraints)
{
  m_pStartNode = pStartNode;
  m_pEndNode = pEndNode;
  m_startPos = startPos;
  m_endPos = endPos;
	m_extraConstraints=extraConstraints;
}


//====================================================================
// GetNodePos
//====================================================================
const Vec3 &CStandardHeuristic::GetNodePos(const GraphNode &node) const
{
  if (&node == m_pStartNode)
    return m_startPos;
  else if (&node == m_pEndNode)
    return m_endPos;
  else return node.GetPos();
}


//====================================================================
// SetRequesterProperties
//====================================================================
void CStandardHeuristic::SetProperties(const PathfindingHeuristicProperties &properties) 
{
  m_properties = properties;

  m_fMinExtraFactor = std::numeric_limits<float>::max();
  if ( (m_properties.agentproperties.navCapMask & IAISystem::NAV_TRIANGULAR) &&
    m_properties.agentproperties.triangularResistanceFactor < m_fMinExtraFactor)
    m_fMinExtraFactor = properties.agentproperties.triangularResistanceFactor;

  if ( (properties.agentproperties.navCapMask & IAISystem::NAV_WAYPOINT_HUMAN) &&
    properties.agentproperties.waypointResistanceFactor < m_fMinExtraFactor)
    m_fMinExtraFactor = properties.agentproperties.waypointResistanceFactor;

  if ( (properties.agentproperties.navCapMask & IAISystem::NAV_WAYPOINT_3DSURFACE) &&
    properties.agentproperties.waypointResistanceFactor < m_fMinExtraFactor)
    m_fMinExtraFactor = properties.agentproperties.waypointResistanceFactor;

  if ( (properties.agentproperties.navCapMask & IAISystem::NAV_FLIGHT) &&
    properties.agentproperties.flightResistanceFactor < m_fMinExtraFactor)
    m_fMinExtraFactor = properties.agentproperties.flightResistanceFactor;

  if ( (properties.agentproperties.navCapMask & IAISystem::NAV_VOLUME) &&
    properties.agentproperties.volumeResistanceFactor < m_fMinExtraFactor)
    m_fMinExtraFactor = properties.agentproperties.volumeResistanceFactor;

  if ( (properties.agentproperties.navCapMask & IAISystem::NAV_ROAD) &&
    properties.agentproperties.roadResistanceFactor < m_fMinExtraFactor)
    m_fMinExtraFactor = properties.agentproperties.roadResistanceFactor;

}

//====================================================================
// EstimateCost
//====================================================================
float CStandardHeuristic::EstimateCost(const AStarSearchNode & node0, const AStarSearchNode & node1) const
{
  if (!gAIEnv.CVars.UseSimplePathfindingHeuristic)
	{
		Vec3 delta = (GetNodePos(*node1.graphNode) - GetNodePos(*node0.graphNode));
		delta.z *= m_properties.agentproperties.zScale;
		return (1.0f + m_fMinExtraFactor) * delta.GetLength();
	}
  else
	{
		return Distance::Point_Point(GetNodePos(*node1.graphNode), GetNodePos(*node0.graphNode));
	}
}

//====================================================================
// CalculateCost
//====================================================================
float CStandardHeuristic::CalculateCost(const CGraph * graph,
																				const AStarSearchNode & node0, unsigned linkIndex0,
																				const AStarSearchNode & node1,
																				const TNavigationBlockers& navigationBlockers) const
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_AI );

	const CGraphLinkManager& linkManager = graph->GetLinkManager();

  uint32 link = linkIndex0;
  if (linkManager.GetRadiusInCm(link) < NavGraphUtils::InCentimeters(m_properties.agentproperties.radius))
    return -1.0f;

	if (!(node1.graphNode->navType & m_properties.agentproperties.navCapMask))
    return -1.0f;

	Vec3 node0Pos = GetNodePos(*node0.graphNode);
	Vec3 node1Pos = GetNodePos(*node1.graphNode);
	
  IF (gAIEnv.CVars.UseSimplePathfindingHeuristic, 0)
		return (node1Pos - node0Pos).GetLength();

	if (linkManager.GetMaxWaterDepthInCm(link) > NavGraphUtils::InCentimeters(m_properties.agentproperties.maxWaterDepth))
    return -1.0f;

  if (linkManager.GetMinWaterDepthInCm(link) < NavGraphUtils::InCentimeters(m_properties.agentproperties.minWaterDepth))
    return -1.0f;

  for (int iExtraConstraint = 0 ; iExtraConstraint < m_extraConstraints.size() ; ++iExtraConstraint)
  {
    const PathfindingExtraConstraint &extraConstraint = m_extraConstraints[iExtraConstraint];
    switch (extraConstraint.type)
    {
    case PathfindingExtraConstraint::ECT_MAXCOST:
      {
        if (extraConstraint.constraint.maxCost.maxCost < node0.fCostFromStart)
          return -1.0f;
      }
      break;
    case PathfindingExtraConstraint::ECT_MINDISTFROMPOINT:
      {
        Vec3 point(extraConstraint.constraint.minDistFromPoint.px,
          extraConstraint.constraint.minDistFromPoint.py,
          extraConstraint.constraint.minDistFromPoint.pz);
				float distToPointSq = Distance::Point_PointSq(point, node0Pos);
        if (distToPointSq > extraConstraint.constraint.minDistFromPoint.minDistSq)
          return -1.0f;
      }
      break;
		case PathfindingExtraConstraint::ECT_AVOIDSPHERE:
			{
				Vec3 p(extraConstraint.constraint.avoidSphere.px,
					extraConstraint.constraint.avoidSphere.py,
					extraConstraint.constraint.avoidSphere.pz);
				Lineseg lineseg(node0Pos, node1Pos);
				float distToPointSq;
				float t;
				if (node0.graphNode->navType == IAISystem::NAV_TRIANGULAR)
					distToPointSq = Distance::Point_Lineseg2DSq(p, lineseg, t);
				else
					distToPointSq = Distance::Point_LinesegSq(p, lineseg, t);
				if (distToPointSq < extraConstraint.constraint.avoidSphere.minDistSq)
					return -1.0f;
			}
			break;
		case PathfindingExtraConstraint::ECT_AVOIDCAPSULE:
			{
				Vec3 p(extraConstraint.constraint.avoidCapsule.px,
					extraConstraint.constraint.avoidCapsule.py,
					extraConstraint.constraint.avoidCapsule.pz);
				Vec3 q(extraConstraint.constraint.avoidCapsule.qx,
					extraConstraint.constraint.avoidCapsule.qy,
					extraConstraint.constraint.avoidCapsule.qz);

				Lineseg lineseg(node0Pos, node1Pos);

				float distToLineSegSq;
				if (node0.graphNode->navType == IAISystem::NAV_TRIANGULAR)
				{
					p.z = 0;
					q.z = 0;
					lineseg.start.z = 0;
					lineseg.end.z = 0;
				}
				distToLineSegSq = Distance::Lineseg_LinesegSq(lineseg, Lineseg(p, q), (float*)0, (float*)0);
				if (distToLineSegSq < extraConstraint.constraint.avoidCapsule.minDistSq)
					return -1.0f;
			}
			break;
		default:
      AIWarning("Unhandled constraint type %d", extraConstraint.type);
      break;
    }
  }

	Vec3 delta = (node1Pos - node0Pos);

  // TODO Danny should these really be 0?! It overrides the params in the lua files, and
  // also maybe invalidates the calculation in EstimateCost
  const float fResistanceFactorTriangularNoWater = 0.5f;
  const float fResistanceFactorTriangularWater = 1.0f;
  const float fResistanceFactorWaypoint = 0.5f;
  const float fResistanceFactorFlight = 0.0f;
  const float fResistanceFactorVolume = 0.0f;
  const float fResistanceFactorRoad = 0.0f;
  const float fResistanceFactorSmartObject = 0.0f;

	const GraphNode* nodes[2] = {node0.graphNode, node1.graphNode};
  float resistanceFactor[2] = {0.0f, 0.0f};
  for (unsigned i = 0 ; i < 2 ; ++i)
  {
    const GraphNode* node = nodes[i];
    switch (node->navType)
    {
    case IAISystem::NAV_TRIANGULAR: 
      resistanceFactor[i] = fResistanceFactorTriangularNoWater * m_properties.agentproperties.triangularResistanceFactor;
      if (linkManager.GetMaxWaterDepthInCm(link) > 150)
        resistanceFactor[i] += fResistanceFactorTriangularWater * (m_properties.agentproperties.triangularResistanceFactor + 2);
      else if (linkManager.GetMaxWaterDepthInCm(link) > 0)
        resistanceFactor[i] += fResistanceFactorTriangularWater * (m_properties.agentproperties.triangularResistanceFactor + 1);
      break;
    case IAISystem::NAV_WAYPOINT_HUMAN: 
      resistanceFactor[i] = fResistanceFactorWaypoint * m_properties.agentproperties.waypointResistanceFactor;
      break;
    case IAISystem::NAV_WAYPOINT_3DSURFACE: 
      resistanceFactor[i] = fResistanceFactorWaypoint * m_properties.agentproperties.waypointResistanceFactor;
      break;
    case IAISystem::NAV_FLIGHT: 
      resistanceFactor[i] = fResistanceFactorFlight * m_properties.agentproperties.flightResistanceFactor;
      break;
    case IAISystem::NAV_VOLUME: 
      resistanceFactor[i] = fResistanceFactorVolume * m_properties.agentproperties.volumeResistanceFactor;
      break;
    case IAISystem::NAV_ROAD: 
      resistanceFactor[i] = fResistanceFactorRoad * m_properties.agentproperties.roadResistanceFactor;
      break;
    case IAISystem::NAV_SMARTOBJECT: 
      resistanceFactor[i] = fResistanceFactorSmartObject * 1.0f;
      break;
    }
  }
  float resFactor = max(resistanceFactor[0], resistanceFactor[1]);

  int nBuildingID0 = nodes[0]->navType & (IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE) ? nodes[0]->GetWaypointNavData()->nBuildingID : -2;
  int nBuildingID1 = nodes[1]->navType & (IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE) ? nodes[1]->GetWaypointNavData()->nBuildingID : -2;

  const float deltaZ = 1.0f;
  float blockerCost = 0.0f;
  float blockerMult = 0.0f;
	float costMult = 1.0f;

	static int maxBlockers = 0;
	if (navigationBlockers.size() > maxBlockers)
	{
		maxBlockers = navigationBlockers.size();
		if (maxBlockers > 10)
			AIWarning ("Too many (%d) navigation blockers passed to CalculateCost()", maxBlockers);
	}

  if (nodes[0]->navType == IAISystem::NAV_SMARTOBJECT && nodes[1]->navType == IAISystem::NAV_SMARTOBJECT)
  {
		// TODO(marcio): Make this work with normal entities.
		// Perhaps with an ISmartObjectUser interface.

		// UGLY:
		CAIObject *pRequester=0;
		if (m_properties.pAgent)
			if (IEntity*pEntity=m_properties.pAgent->GetPathAgentEntity())
				if (IAIObject *pAIObject=pEntity->GetAI())
					pRequester=pAIObject->CastToCAIActor();

    resFactor = gAIEnv.pSmartObjectManager->GetSmartObjectLinkCostFactor(nodes, pRequester, &costMult);
    if ( resFactor < 0.0f )
      return -1.0f;
  }
	else if(nodes[0]->navType == IAISystem::NAV_CUSTOM_NAVIGATION && nodes[1]->navType == IAISystem::NAV_CUSTOM_NAVIGATION)
	{
		resFactor = gAIEnv.pNavigation->GetCustomNavRegion()->GetCustomLinkCostFactor(nodes, m_properties);
	}
  else if (nBuildingID0 >= 0 || nBuildingID1 >= 0)
  {
    // human or 3dsurface waypoint
		Lineseg lineseg(node0Pos, node1Pos);
    unsigned nBlockers = navigationBlockers.size();
    for (unsigned i = 0 ; i < nBlockers ; ++i)
    {
      const SNavigationBlocker& navBlocker = navigationBlockers[i];
      bool useBlocker = false;
      if (navBlocker.restrictedLocation)
      {
        if (!(navBlocker.navType & (IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE)))
          continue;
        if (navBlocker.location.waypoint.nBuildingID == nBuildingID0 ||
          navBlocker.location.waypoint.nBuildingID == nBuildingID1)
          useBlocker = true;
      }
      else
      {
        useBlocker = true;
      }
      if (useBlocker)
      {
        float t = 0.0f;
        float d = Distance::Point_LinesegSq(navBlocker.sphere.center, lineseg, t);
        Vec3 pos = lineseg.GetPoint(t);
        if (pos.z > navBlocker.sphere.center.z - deltaZ && pos.z < navBlocker.sphere.center.z + deltaZ)
        {
          d = navBlocker.navType == IAISystem::NAV_WAYPOINT_HUMAN ? Distance::Point_Point2DSq(pos, navBlocker.sphere.center) : Distance::Point_PointSq(pos, navBlocker.sphere.center);
          if (d < square(navBlocker.sphere.radius))
          {
            if (navBlocker.costMultMod < 0.0f)
              return -1.0f;
            float scale = navBlocker.radialDecay ? square(square(1.0f - cry_sqrtf(d) / navBlocker.sphere.radius)) : 1.0f;
            if (navBlocker.directional)
            {
              /// reduce costs that go away from point
              Vec3 segDir = (lineseg.end - lineseg.start).GetNormalizedSafe();
              Vec3 dirToBlocker = (navBlocker.sphere.center - 0.5f * (lineseg.start + lineseg.end)).GetNormalizedSafe();
              float dirScale = 1.0f - abs(segDir.Dot(dirToBlocker));
              scale *= dirScale;
            }
            blockerCost += scale * navBlocker.costAddMod * (1.0f +  m_properties.agentproperties.waypointResistanceFactor);
            blockerMult += scale * navBlocker.costMultMod;
          }
        }
      }
    }
  }
  else if (nodes[0]->navType == IAISystem::NAV_VOLUME || nodes[1]->navType == IAISystem::NAV_VOLUME)
  {
#ifdef DYNAMIC_3D_NAVIGATION
    // do a collision test to handle dynamic/moving obstacles. Assume that static geometry has already been 
    // accounted for so we just need to check dynamic objects (cheaper). The question is whether it's better
    // to do this here or in background processing. If here, then we should really cache the result.
    bool intersect = 
      GetAISystem()->GetVolumeNavRegion()->PathSegmentWorldIntersection(node0.GetPos(), link.vEdgeCenter, m_properties.radius, false, true, AICE_DYNAMIC) ||
      GetAISystem()->GetVolumeNavRegion()->PathSegmentWorldIntersection(link.vEdgeCenter, node1.GetPos(), m_properties.radius, false, true, AICE_DYNAMIC);
    if (intersect)
      return -1;
#endif

		Lineseg lineseg(node0Pos, node1Pos);
    unsigned nBlockers = navigationBlockers.size();
    for (unsigned i = 0 ; i < nBlockers ; ++i)
    {
      const SNavigationBlocker& navBlocker = navigationBlockers[i];
      bool useBlocker = false;
      if (navBlocker.restrictedLocation)
      {
        if (navBlocker.navType != IAISystem::NAV_VOLUME)
          continue;
      }
      else
      {
        useBlocker = true;
      }
      if (useBlocker)
      {
        float t = 0.0f;
        float d = Distance::Point_LinesegSq(navBlocker.sphere.center, lineseg, t);
        if (d < square(navBlocker.sphere.radius))
        {
          float scale = navBlocker.radialDecay ? square(square(1.0f - cry_sqrtf(d) / navBlocker.sphere.radius)) : 1.0f;
          if (navBlocker.directional)
          {
            /// reduce costs that go away from point
            Vec3 segDir = (lineseg.end - lineseg.start).GetNormalizedSafe();
            Vec3 dirToBlocker = (navBlocker.sphere.center - 0.5f * (lineseg.start + lineseg.end)).GetNormalizedSafe();
            float dirScale = 1.0f - abs(segDir.Dot(dirToBlocker));
            scale *= dirScale;
          }
          blockerCost += scale * navBlocker.costAddMod * (1.0f +  m_properties.agentproperties.volumeResistanceFactor);
          blockerMult += scale * navBlocker.costMultMod;
        }
      }
    }
  }
  else if (nodes[0]->navType == IAISystem::NAV_TRIANGULAR || nodes[1]->navType == IAISystem::NAV_TRIANGULAR)
  {
		Lineseg lineseg(node0Pos, node1Pos);
    unsigned nBlockers = navigationBlockers.size();
    for (unsigned i = 0 ; i < nBlockers ; ++i)
    {
      const SNavigationBlocker& navBlocker = navigationBlockers[i];
      if (!navBlocker.restrictedLocation)
      {
        float t = 0.0f;
        float d = Distance::Point_LinesegSq(navBlocker.sphere.center, lineseg, t);
        if (d < square(navBlocker.sphere.radius))
        {
          if (navBlocker.costMultMod < 0.0f)
            return -1.0f;
          float scale = navBlocker.radialDecay ? square(square(1.0f - cry_sqrtf(d) / navBlocker.sphere.radius)) : 1.0f;
          if (navBlocker.directional)
          {
            /// reduce costs that go away from point
            Vec3 segDir = (lineseg.end - lineseg.start).GetNormalizedSafe();
            Vec3 dirToBlocker = (navBlocker.sphere.center - 0.5f * (lineseg.start + lineseg.end)).GetNormalizedSafe();
            float dirScale = 1.0f - abs(segDir.Dot(dirToBlocker));
            scale *= dirScale;
          }
          blockerCost += scale * navBlocker.costAddMod * (1.0f +  m_properties.agentproperties.triangularResistanceFactor);
          blockerMult += scale * navBlocker.costMultMod;
        }
      }
    }
  }

  // increase slightly the cost of paths that deviate from the direct line - really just to bias the result where
  // there are multiple routes that are equivalent before beautification
  Lineseg line(m_startPos, m_endPos);
  float junk;
	float offsetSq = Distance::Point_LinesegSq(node0Pos, line, junk);
  const float maxTweak = 0.1f;
  const float maxTweakDistSq = square(50.0f);
  Limit(offsetSq, 0.0f, maxTweakDistSq);
  float offsetFactor = maxTweak * offsetSq * (1.0f / maxTweakDistSq);

  float extraLinkFactor = gAIEnv.pNavigation->GetExtraLinkCost(node0Pos, node1Pos);

	if (extraLinkFactor < 0.0f)
	{
			return -1.0f;
		}

  float exposureFactor = m_properties.agentproperties.exposureFactor * linkManager.GetExposure(link);
  delta.z *= m_properties.agentproperties.zScale;
  float length = delta.GetLength();
  float totalCost = blockerCost + (1.0f + resFactor + exposureFactor + extraLinkFactor + blockerMult + offsetFactor) * length;

  return totalCost * costMult;
}
